/*
 * RobotDrive2012.hpp
 *
 *  Created on: Feb 11, 2012
 *      Author: trwulff
 */

/*
 * Description:
 */

#include "Constants.hpp"
#include "WPILib.h"

#define DEFAULTP 800
#define DEFAULTI 0
#define DEFAULTD 10
#define ENCODER_LPR 250 // 250 lpr in quadrature mode (4x)

#ifndef ROBOTDRIVE2012_HPP_
#define ROBOTDRIVE2012_HPP_


class RobotDrive2012
{
public:
	/*
	 * Default constructor
	 */
	RobotDrive2012(UINT8 LeftMotorChannel=2,UINT8 RightMotorChannel=3,float P=DEFAULTP,float I=DEFAULTI,float D=DEFAULTD,UINT16 EncoderLPR=ENCODER_LPR);

	/*
	 * Default destructor
	 */
	~RobotDrive2012();
	void SetPID(double P,double I,double D);
	void ArcadeDrive(float FwdRev, float RtLft);
	void SetEncoderLPR(UINT16 LPR);
	void SetSafetyEnabled (bool safetyEnabled);
	void SetExpiration (float timeout);
	void Drive(float outputMagnitude, float curve);
	void SetNeutral(CANJaguar::NeutralMode mode=CANJaguar::kNeutralMode_Coast);
	void PivotDrive (float leftDriveValue, float rightDriveValue);
	float Max(float V1,float V2);
	float Limit(float);
	void Printf();
	void ChangeToPosition();
	void SetPosition(float Position);
	CANJaguar Jag1, Jag2;

protected:

private:
//	RobotDrive Drive2012;
	float PP,II,DD;
	UINT16 LLPR;
	int iCount;
};
#endif
